%%%%%Determines the height of the non-stance foot over time ONLY in terms
%%%%%of the canonical functions.  No integration needed.  From this the
%%%%%first time of impact can be determined.  


function ret = height(t,a,p0)

ret = l_sl(t,a)*cos(theta_sl(t,a,p0)) - l_nsl(t,a)*cos(theta_nsl(t,a));

end



function ret = theta_nsl(t,a)

ret = atan(yd_ns_slope(t,a));

end


function ret = theta_sl(t,a,p0)

p_hip = yd_hip_pos(t,a)+ p0;

ret = asin(p_hip/l_sl(t,a));

end



function ret = l_nsl(t,a)

Lc = 43561/100000;
Lt = 40134/100000;

ret = sqrt(Lt^2+Lc^2 - 2*Lt*Lc*cos(pi-yd_ns_knee(t,a)));


end




function ret = l_sl(t,a)

Lc = 43561/100000;
Lt = 40134/100000;

ret = sqrt(Lt^2+Lc^2 - 2*Lt*Lc*cos(pi-yd_s_knee(t,a)));

end



%%%%%%%%%%%%%% Canonical human functions

function ret = yd_ns_slope(t,a)

ret =exp(-a(2,4)*t).*(a(2,1)*cos(a(2,2)*t)+a(2,3)*sin(a(2,2)*t))+a(2,5);

end

function ret = yd_hip_pos(t,a)

ret = a(1,1)*t;

end

function ret = yd_s_knee(t,a)

ret = exp(-a(3,4)*t).*(a(3,1)*cos(a(3,2)*t)+a(3,3)*sin(a(3,2)*t))+a(3,5);

end

function ret = yd_ns_knee(t,a)

ret = exp(-a(4,4)*t).*(a(4,1)*cos(a(4,2)*t)+a(4,3)*sin(a(4,2)*t))+a(4,5);

end
